package novemberBot.mapmaking;

import novemberBot.FactoryFloorRobot;
/**for use with dijkstra's pathfinding algorithm
 * represents a map of vertices organized into a fixed size arraylist
 * @author Antoine Lennartz*/
public class Graph {
	/**vertices array*/
	public Vertice[][] vertices;
	public int[] pos = new int[2];
	private int size;
	
	/**Constructor
	 * @param size - dimensions of map*/
	public Graph(int size){
		this.size=size;
		vertices = new Vertice[size][size];
		linkVertices();
	}
	/**normally only called once from the constructor */
	private void linkVertices(){
		for(int i=0; i<size; i++){
			for(int j=0; j<size; j++){
				vertices[i][j]=new Vertice();
			}
		}
		for(int i=0; i<size; i++){
			for(int j=0; j<size; j++){
				vertices[i][j].coords[0]=i;
				vertices[i][j].coords[1]=j;
				if(i!=size-1)vertices[i][j].east=vertices[i+1][j];
				if(i!=0)vertices[i][j].west=vertices[i-1][j];
				if(j!=size-1)vertices[i][j].north=vertices[i][j+1];
				if(j!=0)vertices[i][j].south=vertices[i][j-1];
			}
		}
	}
	
	/**marks the vertices containing obstacles as visited so that they are ignored
	 * when computing the path*/
	public void reBuild(byte[][] map){
		for(int i=0; i<size; i++){
			for(int j=0; j<size; j++){
				vertices[i][j].visited=false;
				vertices[i][j].score=-1;
				if(i==0 || j==0 || i==size-1 || j==size-1 ||isObstacle(map[i][j])){
					vertices[i][j].score=127;
					vertices[i][j].visited=true;
				}
			}
		}
		reScore();
	}
	/**scores all the nodes of the path starting at the start position
	 * weight is purely based on distance*/
	private void reScore(){
		double[] cPos = new double[3];
		FactoryFloorRobot.getInstance().getOdometer().getPosition(cPos);
		int[] mapPos = Map.mapCoords(cPos);
		vertices[mapPos[0]][mapPos[1]].score=0;
		scoreNeighbours(vertices[mapPos[0]][mapPos[1]]);
		
	}
	/**recursively scores the map starting from Vertice V*/
	private void scoreNeighbours(Vertice v){
		if(v.north!=v && v.north.score!=127 && (v.north.score>v.score+1 || v.north.score==-1)){
			v.north.score=(byte) (v.score+1);
			if(v.north.visited)v.north.visited=false;
		}
		if(v.south!=v && v.south.score!=127 && (v.south.score>v.score+1 || v.south.score==-1)){
			v.south.score=(byte) (v.score+1);
			if(v.south.visited)v.south.visited=false;
		}
		if(v.east!=v && v.east.score!=127 && (v.east.score>v.score+1 || v.east.score==-1)){
			v.east.score=(byte) (v.score+1);
			if(v.east.visited)v.east.visited=false;
		}
		if(v.west!=v && v.west.score!=127 && (v.west.score>v.score+1 || v.west.score==-1)){
			v.west.score=(byte) (v.score+1);
			if(v.west.visited)v.west.visited=false;
		}
		v.visited=true;
		if(!v.north.visited && v.north!=v) scoreNeighbours(v.north);
		if(!v.south.visited && v.south!=v) scoreNeighbours(v.south);
		if(!v.east.visited && v.east!=v) scoreNeighbours(v.east);
		if(!v.west.visited && v.west!=v) scoreNeighbours(v.west);
	}
	/**determines weather b is an obstacle
	 * @param b - the byte to compare*/
	private boolean isObstacle(byte b){
		if(b>Map.OCCUPIED) return true;
		return false;
	}
	
}
